
// import haxe.ds.*;
// import oimo.collision.*;
// import oimo.collision.broadphase.*;
// import oimo.collision.broadphase.bruteforce.*;
// import oimo.collision.broadphase.bvh.*;
// import oimo.collision.geometry.*;
// import oimo.collision.narrowphase.detector.gjkepa.*;
// import oimo.common.*;
// import oimo.dynamics.callback.*;
// import oimo.dynamics.common.*;
// import oimo.dynamics.constraint.*;
// import oimo.dynamics.constraint.contact.*;
// import oimo.dynamics.constraint.joint.*;
// import oimo.dynamics.rigidbody.*;
// import oimo.m.*;

import { RigidBody } from "./rigidbody/RigidBody";
import { Joint } from "./constraint/joint/Joint";
import { BroadPhase } from "../collision/broadphase/BroadPhase";
import { ContactManager } from "./ContactManager";
import { Vec3 } from "../common/Vec3";
import { TimeStep } from "./TimeStep";
import { ConstraintSolver } from "./constraint/ConstraintSolver";
import { Setting } from "../common/Setting";
import { M } from "../common/M";
import { RigidBodyType } from "./rigidbody/RigidBodyType";
import { ContactLink } from "./ContactLink";
import { JointLink } from "./constraint/joint/JointLink";
import { Shape } from "./rigidbody/Shape";
import { Transform } from "../common/Transform";
import { Geometry } from "../collision/geometry/Geometry";
import { GeometryType } from "../collision/geometry/GeometryType";
import { SphereGeometry } from "../collision/geometry/SphereGeometry";
import { BoxGeometry } from "../collision/geometry/BoxGeometry";
import { CapsuleGeometry } from "../collision/geometry/CapsuleGeometry";
import { Aabb } from "../collision/geometry/Aabb";
import { Contact } from "./Contact";
import { ContactConstraint } from "./constraint/contact/ContactConstraint";
import { ManifoldPoint } from "./constraint/contact/ManifoldPoint";
import { MathUtil } from "../common/MathUtil";
import { ConvexGeometry } from "../collision/geometry/ConvexGeometry";
import { BroadPhaseProxyCallback } from "../collision/broadphase/BroadPhaseProxyCallback";
import { RayCastHit } from "../collision/geometry/RayCastHit";
import { BroadPhaseType } from "../collision/broadphase/BroadPhaseType";
import { BruteForceBroadPhase } from "../collision/broadphase/bruteforce/BruteForceBroadPhase";
import { BvhTree } from "../collision/broadphase/bvh/BvhTree";
import { BvhNode } from "../collision/broadphase/bvh/BvhNode";
import { Mat3 } from "../common/Mat3";
import { RotationalLimitMotor } from "./constraint/joint/RotationalLimitMotor";
import { TranslationalLimitMotor } from "./constraint/joint/TranslationalLimitMotor";
import { Quat } from "../common/Quat";
import { Island } from "./Island";
import { BvhBroadPhase } from "../collision/broadphase/bvh/BvhBroadPhase";
import { Pool } from "../common/Pool";
import { RayCastCallback } from "./callback/RayCastCallback";
import { AabbTestCallback } from "./callback/AabbTestCallback";
import { Proxy } from "../collision/broadphase/Proxy";
import { GjkEpa } from "../collision/narrowphase/detector/gjkepa/GjkEpa";

/**
 * The physics simulation world. This manages entire the dynamic simulation. You can add
 * rigid bodies and joints to the world to simulate them.
 */
export class World {
	_rigidBodyList: RigidBody;
	_rigidBodyListLast: RigidBody;

	_jointList: Joint;
	_jointListLast: Joint;

	_broadPhase: BroadPhase;
	_contactManager: ContactManager;

	_numRigidBodies: number;
	_numJoints: number;
	_numShapes: number;
	_numIslands: number;

	_numVelocityIterations: number;
	_numPositionIterations: number;

	_gravity: Vec3;

	_timeStep: TimeStep;
	_island: Island;
	_rigidBodyStack: Array<RigidBody>;
	_solversInIslands: Array<ConstraintSolver>;
	_numSolversInIslands: number = 0;

	_rayCastWrapper: RayCastWrapper;
	_convexCastWrapper: ConvexCastWrapper;
	_aabbTestWrapper: AabbTestWrapper;

	_pool: Pool;

	_shapeIdCount: number;

	/**
	 * Creates a new physics world, with broad-phase collision detection algorithm `broadPhaseType` and
	 * gravitational acceleration `gravity`.
	 */
	constructor(broadPhaseType: number = BroadPhaseType.BVH, gravity: Vec3 = null) {
		switch (broadPhaseType) {
			case BroadPhaseType.BRUTE_FORCE:
				this._broadPhase = new BruteForceBroadPhase();
			case BroadPhaseType.BVH:
			default:
				this._broadPhase = new BvhBroadPhase();
				break

		}
		this._contactManager = new ContactManager(this._broadPhase);

		if (gravity == null) gravity = new Vec3(0, -9.80665, 0);
		this._gravity = gravity.clone();

		this._rigidBodyList = null;
		this._rigidBodyListLast = null;

		this._jointList = null;
		this._jointListLast = null;

		this._numRigidBodies = 0;
		this._numShapes = 0;
		this._numJoints = 0;
		this._numIslands = 0;

		this._numVelocityIterations = 10;
		this._numPositionIterations = 5;

		this._rayCastWrapper = new RayCastWrapper();
		this._convexCastWrapper = new ConvexCastWrapper();
		this._aabbTestWrapper = new AabbTestWrapper();

		this._island = new Island();
		this._solversInIslands = new Array<ConstraintSolver>(Setting.islandInitialConstraintArraySize);
		this._rigidBodyStack = new Array<RigidBody>(Setting.islandInitialRigidBodyArraySize);

		this._timeStep = new TimeStep();

		this._pool = new Pool();


		this._shapeIdCount = 0;
	}

	public _updateContacts(): void {
		this._contactManager._updateContacts();
		this._contactManager._updateManifolds();
	}

	public _solveIslands(): void {
		// wake up all rigid bodies if sleeping is disabled
		if (Setting.disableSleeping) {
			var b: RigidBody = this._rigidBodyList;
			while (b != null) {
				b._sleeping = false;
				b._sleepTime = 0;
				b = b._next;
			}
		}

		// build and solve islands
		this._numIslands = 0;
		this._island._setGravity(this._gravity);
		var b: RigidBody = this._rigidBodyList;
		this._numSolversInIslands = 0;
		while (b != null) {
			do {
				if (b._addedToIsland || b._sleeping || b._type == RigidBodyType.STATIC) {
					// never be the base of an island
					break;
				}
				if (b._isAlone()) {
					this._island._stepSingleRigidBody(this._timeStep, b);
					this._numIslands++;
					break;
				}

				this.buildIsland(b);

				this._island._step(this._timeStep, this._numVelocityIterations, this._numPositionIterations);
				this._island._clear();
				this._numIslands++;
			} while (false);

			this._contactManager._postSolve();

			// clear island flags
			let bN = this._rigidBodyList;
			while (bN != null) {
				bN._addedToIsland = false;
				M.vec3_zero(bN._force);
				M.vec3_zero(bN._torque);
				bN = bN._next;
			}

			while (this._numSolversInIslands > 0) {
				this._solversInIslands[--this._numSolversInIslands]._addedToIsland = false;
				this._solversInIslands[this._numSolversInIslands] = null;
			}
			b = b._next;
		}
	}

	public buildIsland(base: RigidBody): void {
		// begin DFS
		var stackCount: number = 1;
		this._island._addRigidBody(base);
		this._rigidBodyStack[0] = base;

		while (stackCount > 0) {
			// pop a rigid body
			var rb: RigidBody = this._rigidBodyStack[--stackCount];
			this._rigidBodyStack[stackCount] = null;

			// stop searching deeper
			if (rb._type == RigidBodyType.STATIC) {
				continue;
			}

			// searching contacts
			var cl: ContactLink = rb._contactLinkList;
			while (cl != null) {
				// ignore if not touching
				var cc: ContactConstraint = cl._contact._contactConstraint;
				var ccs: ConstraintSolver = cl._contact._contactConstraint._solver;
				if (cc.isTouching() && !ccs._addedToIsland) {

					// add to constraint array (to clear island flag later)
					this._solversInIslands[this._numSolversInIslands++] = ccs;

					// add to island
					this._island._addConstraintSolver(ccs, cc._positionCorrectionAlgorithm);

					// push the other rigid body if not added
					var other: RigidBody = cl._other;
					if (!other._addedToIsland) {
						this._island._addRigidBody(other);
						this._rigidBodyStack[stackCount++] = other;
					}
				}
				cl = cl._next;
			}

			// searching joints
			var jl: JointLink = rb._jointLinkList;
			while (jl != null) {
				var j: Joint = jl._joint;
				var js: ConstraintSolver = j._solver;
				if (!js._addedToIsland) {

					// add to constraint array (to clear island flag later)
					this._solversInIslands[this._numSolversInIslands++] = js;

					// add to island
					this._island._addConstraintSolver(js, j._positionCorrectionAlgorithm);

					// push the other rigid body if not added
					var other: RigidBody = jl._other;
					if (!other._addedToIsland) {
						this._island._addRigidBody(other);
						this._rigidBodyStack[stackCount++] = other;
					}
				}
				jl = jl._next;
			}
		}
	}


	public _addShape(shape: Shape): void {
		shape._proxy = this._broadPhase.createProxy(shape, shape._aabb);
		shape._id = this._shapeIdCount++;

		this._numShapes++;
	}


	public _removeShape(shape: Shape): void {
		this._broadPhase.destroyProxy(shape._proxy);
		shape._proxy = null;
		shape._id = -1;

		// destroy linked contacts
		var cl: ContactLink = shape._rigidBody._contactLinkList;
		while (cl != null) {
			var c: Contact = cl._contact;
			if (c._s1 == shape || c._s2 == shape) {
				cl._other.wakeUp();
				this._contactManager._destroyContact(c);
			}
			cl = cl._next;
		}

		this._numShapes--;
	}

	// --- public ---

	/**
	 * Advances the simulation by the time step `timeStep`.
	 */
	public step(timeStep: number): void {
		if (this._timeStep.dt > 0) {
			this._timeStep.dtRatio = timeStep / this._timeStep.dt;
		}
		this._timeStep.dt = timeStep;
		this._timeStep.invDt = 1 / timeStep;

		this._updateContacts();
		this._solveIslands();
	}

	/**
	 * Adds the rigid body `rigidBody` to the simulation world.
	 */
	public addRigidBody(rigidBody: RigidBody): void {
		if (rigidBody._world != null) {
			throw M.error("A rigid body cannot belong to multiple worlds.");
		}

		// first, add the rigid body to the world
		if (this._rigidBodyList == null) {
			this._rigidBodyList = rigidBody;
			this._rigidBodyListLast = rigidBody;
		} else {
			this._rigidBodyListLast._next = rigidBody;
			rigidBody._prev = this._rigidBodyListLast;
			this._rigidBodyListLast = rigidBody;
		}
		// M.list_push(this._rigidBodyList, this._rigidBodyListLast, _prev, _next, rigidBody);
		rigidBody._world = this;

		// then add the shapes to the world
		var s: Shape = rigidBody._shapeList;
		while (s != null) {
			this._addShape(s);
			s = s._next;
		}

		this._numRigidBodies++;
	}

	/**
	 * Removes the rigid body `rigidBody` from the simulation world.
	 */
	public removeRigidBody(rigidBody: RigidBody): void {
		if (rigidBody._world != this) {
			throw M.error("The rigid body doesn't belong to the world.");
		}
		// first, remove the rigid body from the world
		let prev = rigidBody._prev;
		let next = rigidBody._next;
		if (prev != null) {
			prev._next = next;
		}
		if (next != null) {
			next._prev = prev;
		}
		if (rigidBody == this._rigidBodyList) {
			this._rigidBodyList = next;
		}
		if (rigidBody == this._rigidBodyListLast) {
			this._rigidBodyListLast = prev;
		}
		rigidBody._prev = null;
		rigidBody._next = null;
		// M.list_remove(this._rigidBodyList, this._rigidBodyListLast, _prev, _next, rigidBody);
		rigidBody._world = null;

		// then remove the shapes from the world
		var s: Shape = rigidBody._shapeList;
		while (s != null) {
			this._removeShape(s);
			s = s._next;
		}

		this._numRigidBodies--;
	}

	/**
	 * Adds the joint `joint` to the simulation world.
	 */
	public addJoint(joint: Joint): void {
		if (joint._world != null) {
			throw M.error("A joint cannot belong to multiple worlds.");
		}

		if (this._jointList == null) {
			this._jointList = joint;
			this._jointListLast = joint;
		} else {
			this._jointListLast._next = joint;
			joint._prev = this._jointListLast;
			this._jointListLast = joint;
		}
		// M.list_push(this._jointList, this._jointListLast, _prev, _next, joint);
		joint._world = this;
		joint._attachLinks();
		joint._syncAnchors();

		this._numJoints++;
	}

	/**
	 * Removes the joint `joint` from the simulation world.
	 */
	public removeJoint(joint: Joint): void {
		if (joint._world != this) {
			throw M.error("The joint doesn't belong to the world.");
		}
		let prev = joint._prev;
		let next = joint._next;
		if (prev != null) {
			prev._next = next;
		}
		if (next != null) {
			next._prev = prev;
		}
		if (joint == this._jointList) {
			this._jointList = next;
		}
		if (joint == this._jointListLast) {
			this._jointListLast = prev;
		}
		joint._prev = null;
		joint._next = null;
		// M.list_remove(this._jointList, this._jointListLast, _prev, _next, joint);
		joint._world = null;
		joint._detachLinks();

		this._numJoints--;
	}

	/**
	 * Performs a ray casting. `callback.process` is called for all shapes the ray
	 * from `begin` to `end` hits.
	 */
	public rayCast(begin: Vec3, end: Vec3, callback: RayCastCallback): void {
		this._rayCastWrapper.begin.copyFrom(begin);
		this._rayCastWrapper.end.copyFrom(end);
		this._rayCastWrapper.callback = callback;

		this._broadPhase.rayCast(begin, end, this._rayCastWrapper);
	}

	/**
	 * Performs a convex casting. `callback.process` is called for all shapes the convex geometry
	 * `convex` hits. The convex geometry translates by `translation` starting from the beginning
	 * transform `begin`.
	 */
	public convexCast(convex: ConvexGeometry, begin: Transform, translation: Vec3, callback: RayCastCallback): void {
		this._convexCastWrapper.convex = convex;
		this._convexCastWrapper.begin.copyFrom(begin);
		this._convexCastWrapper.translation.copyFrom(translation);
		this._convexCastWrapper.callback = callback;

		this._broadPhase.convexCast(convex, begin, translation, this._convexCastWrapper);
	}

	/**
	 * Performs an AABB query. `callback.process` is called for all shapes that their
	 * AABB and `aabb` intersect.
	 */
	public aabbTest(aabb: Aabb, callback: AabbTestCallback): void {
		this._aabbTestWrapper._aabb.copyFrom(aabb);
		this._aabbTestWrapper._callback = callback;

		this._broadPhase.aabbTest(aabb, this._aabbTestWrapper);
	}

	/**
	 * Returns the list of the rigid bodies added to the world.
	 */
	public getRigidBodyList(): RigidBody {
		return this._rigidBodyList;
	}

	/**
	 * Returns the list of the joints added to the world.
	 */
	public getJointList(): Joint {
		return this._jointList;
	}

	/**
	 * Returns the broad-phase collision detection algorithm.
	 */
	public getBroadPhase(): BroadPhase {
		return this._broadPhase;
	}

	/**
	 * Returns the contact manager.
	 */
	public getContactManager(): ContactManager {
		return this._contactManager;
	}

	/**
	 * Returns the number of the rigid bodies added to the world.
	 */
	public getNumRigidBodies(): number {
		return this._numRigidBodies;
	}

	/**
	 * Returns the number of the joints added to the world.
	 */
	public getNumJoints(): number {
		return this._numJoints;
	}

	/**
	 * Returns the number of the shapes added to the world.
	 */
	public getNumShapes(): number {
		return this._numShapes;
	}

	/**
	 * Returns the number of simulation islands.
	 */
	public getNumIslands(): number {
		return this._numIslands;
	}

	/**
	 * Returns the number of velocity iterations of constraint solvers.
	 */
	public getNumVelocityIterations(): number {
		return this._numVelocityIterations;
	}

	/**
	 * Sets the number of velocity iterations of constraint solvers to `numVelocityIterations`.
	 */
	public setNumVelocityIterations(numVelocityIterations: number): void {
		this._numVelocityIterations = numVelocityIterations;
	}

	/**
	 * Returns the number of position iterations of constraint solvers.
	 */
	public getNumPositionIterations(): number {
		return this._numPositionIterations;
	}

	/**
	 * Sets the number of position iterations of constraint solvers to `numPositionIterations`.
	 */
	public setNumPositionIterations(numPositionIterations: number): void {
		this._numPositionIterations = numPositionIterations;
	}

	/**
	 * Returns the gravitational acceleration of the simulation world.
	 */
	public getGravity(): Vec3 {
		return this._gravity;
	}

	/**
	 * Sets the gravitational acceleration of the simulation world to `gravity`.
	 */
	public setGravity(gravity: Vec3): void {
		this._gravity.copyFrom(gravity);
	}

}

// ray cast wrapper (broadphase -> world)
class RayCastWrapper extends BroadPhaseProxyCallback {
	callback: RayCastCallback;
	begin: Vec3;
	end: Vec3;

	rayCastHit: RayCastHit;

	constructor() {
		super();
		this.rayCastHit = new RayCastHit();

		this.begin = new Vec3();
		this.end = new Vec3();
		this.callback = null;
	}

	public process(proxy: Proxy): void {
		var shape: Shape = proxy.userData as Shape;

		if (shape._geom.rayCast(this.begin, this.end, shape._transform, this.rayCastHit)) {
			this.callback.process(shape, this.rayCastHit);
		}
	}
}

// convex cast wrapper (broadphase -> world)
class ConvexCastWrapper extends BroadPhaseProxyCallback {
	callback: RayCastCallback;
	begin: Transform;
	translation: Vec3;
	convex: ConvexGeometry;

	rayCastHit: RayCastHit;
	zero: Vec3;

	constructor() {
		super();
		this.rayCastHit = new RayCastHit();

		this.begin = new Transform();
		this.translation = new Vec3();
		this.zero = new Vec3();
		this.callback = null;
		this.convex = null;
	}

	public process(proxy: Proxy): void {
		var shape: Shape = proxy.userData as Shape;
		var type: number = shape._geom._type;
		if (type < GeometryType._CONVEX_MIN || type > GeometryType._CONVEX_MAX) return;

		var geom: ConvexGeometry = shape._geom as ConvexGeometry;
		if (GjkEpa.getInstance().convexCast(this.convex, geom, this.begin, shape._transform, this.translation, this.zero, this.rayCastHit)) {
			this.callback.process(shape, this.rayCastHit);
		}
	}
}

// aabb test wrapper (broadphase -> world)
class AabbTestWrapper extends BroadPhaseProxyCallback {
	_callback: AabbTestCallback;
	_aabb: Aabb;

	constructor() {
		super();

		this._aabb = new Aabb();
		this._callback = null;
	}

	public process(proxy: Proxy): void {
		var shape: Shape = proxy.userData as Shape;
		var shapeAabb: Aabb = shape._aabb;

		// check if aabbs overlap again as proxies can be fattened by broadphase
		if (M.aabb_overlap(shapeAabb._min, shapeAabb._max, this._aabb._min, this._aabb._max)) {
			this._callback.process(shape);
		}
	}
}
